#include "Remote.h"
#include "SBUS.h"
#include "Dog.h"

#define CHANNEL_FB sbus_channels[0]
#define CHANNEL_LR sbus_channels[1]
#define CHANNEL_THROTTLE sbus_channels[2]
#define CHANNEL_RESET sbus_channels[3]
#define CHANNEL_STATE sbus_channels[4]
#define CHANNEL_HEIGHT sbus_channels[5]
#define CHANNEL_JUMPANGLE sbus_channels[6]
#define CHANNEL_TROT sbus_channels[7]
#define CHANNEL_STAND sbus_channels[8]
#define CHANNEL_JUMP sbus_channels[9]

#define LIMIT_MIN_MAX(x, min, max) (x) = (((x) <= (min)) ? (min) : (((x) >= (max)) ? (max) : (x)))

static uint8_t isReset;

void Remote_UART_Callback()
{
    /********************************** 拉杆F下拉才能进入遥控模式, 退出立即复位 ***********************************/

    if (CHANNEL_RESET > 1000) // F下拉进入遥控模式
    {
        isReset = 1; // 可以复位
        HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_SET);

        /************************************* 拉杆B下拉站立, 上拉趴下 ******************************************/

        if (CHANNEL_STAND > 1000) // B下拉站立
        {
            /*************************** 行走拉杆D下拉跳跃, 否则拉杆A下拉, 否则站立 *******************************/

            if (CHANNEL_JUMP > 1000) // D下拉跳跃
            {
                /************************************ 跳跃 *************************************/

                /* 右旋钮 */
                global.jumpAngle = (CHANNEL_JUMPANGLE - 200) / 1567.0f * (MAX_JUMPANGLE - MIN_JUMPANGLE) + MIN_JUMPANGLE;

                /* 跳跃第一阶段 */
                if (FSM.currentState != JUMPL)
                {
                    FSM_SetState(JUMPL);
                }
            }
            else if (CHANNEL_TROT > 1000) // A下拉行走
            {
                /************************************ 行走 *************************************/
                float maxS = sqrtf(MAX_LEG * MAX_LEG - global.height * global.height); // 最大步幅
                float maxH = global.height - MIN_LEG;                                  // 最大步高

                /* 移动 */
                /* 前为前 */
                dog.legs[0].S = (float)(CHANNEL_FB - CHANNEL_LR) / 800.0f * maxS;
                dog.legs[1].S = (float)(CHANNEL_FB + CHANNEL_LR - 2048) / 800.0f * maxS;
                dog.legs[2].S = (float)(CHANNEL_FB + CHANNEL_LR - 2048) / 800.0f * maxS;
                dog.legs[3].S = (float)(CHANNEL_FB - CHANNEL_LR) / 800.0f * maxS;

                LIMIT_MIN_MAX(dog.legs[0].S, -maxS, maxS);
                LIMIT_MIN_MAX(dog.legs[1].S, -maxS, maxS);
                LIMIT_MIN_MAX(dog.legs[2].S, -maxS, maxS);
                LIMIT_MIN_MAX(dog.legs[3].S, -maxS, maxS);

                /* 步频 */
                global.throttle = (int32_t)((1800 - CHANNEL_THROTTLE) / 100) / 15.68f * (MAX_THROTTLE - MIN_THROTTLE) + MIN_THROTTLE;
                if (CHANNEL_THROTTLE > 300) // 油门小于300停止
                {
                    global.isWalking = 1;
                }
                else
                {
                    global.isWalking = 0;
                }

                /* 左旋钮 */
                float h = (CHANNEL_HEIGHT - 200) / 1600.0f * maxH; // 设置狗腿步高
                dog.legs[0].H = h;
                dog.legs[1].H = h;
                dog.legs[2].H = h;
                dog.legs[3].H = h;

                /* trot */
                if (FSM.currentState != TROTING)
                {
                    FSM_SetState(TROTING);
                }
                // if (FSM.currentState != WALKING)
                // {
                //     FSM_SetState(WALKING);
                // }
            }
            else
            {
                /************************************ 站立 *************************************/
                if (FSM.currentState != FIXEDSTAND)
                {
                    FSM_SetState(FIXEDSTAND);
                }
            }
        }
        else // B上拉趴下
        {
            /************************************ 趴下 *************************************/
            if (FSM.currentState != FALL)
            {
                FSM_SetState(FALL);
            }
        }
    }
    else // F上拉复位
    {
        /************************************ 复位 *************************************/
        if (isReset)
        {
            HAL_NVIC_SystemReset();
        }
    }
}

inline void Remote_Init()
{
    SBUS_Init(Remote_UART_Callback);
}
